package com.crunch.physics;

import com.crunch.math.MathOps;
import com.crunch.math.Vector2f;

import java.util.*;

/**
 * Evaluates physics queries and updates.
 */
class PhysicsEvaluator {
	public PhysicsEvaluator(PhysicsSimulation simulation) {
		this.simulation = simulation;
		contactManager = new ContactManager(simulation);

		queryBuffer = new ArrayList<HashGrid.Entity>();
		pairQueryBuffer = new ArrayList<HashGrid.CollisionPair>();
		raycastBuffer = new ArrayList<PhysicsRaycast.Result>();
	}

	public void stepSimulation(float dt) {
		if (simulation.simulationLock) {
			throw new IllegalStateException("Simulation locked");
		}

		simulation.simulationLock = true;

		// overview:
		// - integrate forces
		// - step dynamic entities, colliding continuously against world
		// - step movable entities, colliding continuously against dynamic entities
		// - resolve interactions between dynamic entities

		// perform onPreStep() for entities
		for (DynamicPhysicsEntity e : simulation.dynamicEntities) {
			if (e.exists() && e.getEventHandler() != null) {
				e.getEventHandler().onPreStep(e);
			}
		}

		preResolveDynamicInteractions();
		updateContacts();
		integrateForces(dt);
		preStepMovableEntities(dt);
		stepDynamicEntities(dt);
		postStepMovableEntities(dt);
		resolveDynamicInteractions();

		// perform onPostStep() for entities
		for (DynamicPhysicsEntity e : simulation.dynamicEntities) {
			if (e.exists() && e.getEventHandler() != null) {
				e.getEventHandler().onPostStep(e);
			}
		}

		simulation.simulationLock = false;

		// perform delayed entity destruction/creation
		simulation.addQueued();
		simulation.deleteQueued();

		// check contacts so that they are correct post-simulation
		updateContacts();
	}

	private void updateContacts() {
		for (DynamicPhysicsEntity e : simulation.dynamicEntities) {
			if (e.exists()) {
				contactManager.updateContacts(e);
			}
		}
	}

	private void integrateForces(float dt) {
		for (DynamicPhysicsEntity d : simulation.dynamicEntities) {
			if (d.exists()) {
				integrateForces(d, dt);
			}
		}
	}

	private void integrateForces(DynamicPhysicsEntity d, float dt) {
		if (d.eventHandler != null) {
			d.eventHandler.onProcessInput(d, dt);
		}

		d.velocity.addAssign(d.currentRepelAcceleration);
		d.currentRepelAcceleration.setZero();
	}

	private void stepDynamicEntities(float dt) {
		// set movable entity lock because onSquish should not be able to affect movable entities
		simulation.movableEntityLock = true;
		for (DynamicPhysicsEntity e : simulation.dynamicEntities) {
			if (e.exists()) {
				stepDynamicEntity(e, dt);
			}
		}
		simulation.movableEntityLock = false;
	}

	// expands movable entity AABBs to cover velocity
	private void preStepMovableEntities(float dt) {
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		for (MovablePhysicsEntity e : simulation.movableEntities) {
			if (e.exists() && !e.velocity.isZero()) {
				// expand AABB to cover entire swept motion of velocity
				e.getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);
				sweepAabb(aabbMin, aabbMax, e.velocity, aabbMin, aabbMax);
				e.gridEntity.move(aabbMin, aabbMax);
			}
		}
	}

	// sets movable entity new positions
	private void postStepMovableEntities(float dt) {
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		Vector2f velDt = new Vector2f();
		for (MovablePhysicsEntity e : simulation.movableEntities) {
			if (e.exists() && !e.velocity.isZero()) {
				velDt.set(e.velocity);
				velDt.multiplyAssign(dt);
				e.position.addAssign(velDt);
				// revert AABB back to
				e.getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);
				sweepAabb(aabbMin, aabbMax, e.velocity, aabbMin, aabbMax);
				e.gridEntity.move(aabbMin, aabbMax);
			}
		}
	}

	private void stepDynamicEntity(DynamicPhysicsEntity d, float dt) {
		// don't return if velocity is zero - a movable entity might hit us

		// todo stick to floor after step
		boolean wasOnFloor = (d.floorContact != null);
		Vector2f floorVelocity = wasOnFloor ? new Vector2f(d.velocity) : null;
		Vector2f floorNormal = wasOnFloor ? d.floorContact.getNormal() : null;

		Vector2f oldPosition = new Vector2f(d.position);

		final int MAX_ITERATIONS = 10;
		// initially we have all dt remaining
		float t = 0.0f;
		int iterationsRemaining = MAX_ITERATIONS;
		PhysicsMath.SweepResult sweepResult = new PhysicsMath.SweepResult();
		sweepResult.point = new Vector2f();
		sweepResult.normal = new Vector2f();
		Vector2f constrainedVelocity = new Vector2f();
		int[] constraintIndices = new int[2];
		// loop until we either run out of t or iterations
		// we can't stop iterating if velocity is zero because a movable entity might hit us
		while (t < dt && iterationsRemaining > 0) {
			float tRemaining = dt - t;
			// find first intersection with scene
			findFirstIntersection(d, t, tRemaining, sweepResult);

			if (sweepResult.t >= tRemaining) {
				// no intersection found (t = infinite) or intersection was past amount of time we have remaining
				// simply update position directly
				d.position.addAssign(d.velocity.multiply(tRemaining));
				t = dt;
			} else {
				// t < tRemaining: intersection was found, increment up to that point
				t += sweepResult.t;

				// update position
				d.position.addAssign(d.velocity.multiply(sweepResult.t));
			}

			contactManager.clearMovementContacts();
			contactManager.determineContacts(d, t);

			MovablePhysicsEntity squishEntity = null;
			List<ContactManager.MovementContact> movementContacts = contactManager.getUniqueMovementContacts();
			// compute new velocity by constraining to the contacts
			if (computeConstrainedVelocity(d.velocity, constrainedVelocity, constraintIndices)) {
				// determine if we've squished
				// we squish if we are pressed in between two surfaces and need to move a far distance
				if (constraintIndices[0] >= 0 && constraintIndices[1] >= 0) {
					ContactManager.MovementContact mcA = movementContacts.get(constraintIndices[0]);
					ContactManager.MovementContact mcB = movementContacts.get(constraintIndices[1]);
					squishEntity = determineSquish(mcA, mcB);
				}

				if (squishEntity != null) {
					// we got squished
					d.velocity.setZero();
					iterationsRemaining = 0;
				} else {
					// successfully constrained the velocity
					d.velocity.set(constrainedVelocity);
					--iterationsRemaining;
				}
			} else {
				// failed to constrain velocity to contacts - best we can do is stop moving
				d.velocity.setZero();
				iterationsRemaining = 0;

				// if one of the contacts is a movable entity we can call its squish event
				for (ContactManager.MovementContact mc : movementContacts) {
					if (mc.type == Contact.Type.MOVABLE_ENTITY) {
						MovablePhysicsEntity m = (MovablePhysicsEntity) mc.object;
						if (m.eventHandler != null) {
							squishEntity = m;
							break;
						}
					}
				}
			}

			// call squish event if we've been squished
			if (squishEntity != null) {
				Vector2f squishEntityPos = new Vector2f(squishEntity.position);
				squishEntity.position.set(squishEntity.velocity);
				squishEntity.position.multiplyAssign(t).addAssign(squishEntityPos);
				// call the on squish event handler with the new position for the movable entity
				squishEntity.eventHandler.onSquish(squishEntity, d);
				// revert to original position
				squishEntity.position.set(squishEntityPos);
			}
		}

		// update the entity's position in the grid by calling setPosition()
		Vector2f newPosition = new Vector2f(d.position);
		d.position.set(oldPosition);
		d.setPosition(newPosition);

		contactManager.clearMovementContacts();
	}

	private void findFirstIntersection(DynamicPhysicsEntity d, float t, float dt, PhysicsMath.SweepResult sweepResult) {
		// create a bounding box for entire swept motion
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		d.getAabb(aabbMin, aabbMax, simulation.settings.contactThreshold);
		sweepAabb(aabbMin, aabbMax, d.velocity.multiply(dt), aabbMin, aabbMax);

		List<ContactManager.MovementContact> movementContacts = contactManager.getMovementContacts();

		// find the first intersection with a collider
		HashGrid.CollisionAabbSet set = simulation.hashGrid.getCollisionAabbSet(
				(1 << PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE) | (1 << PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE),
				aabbMin, aabbMax);
		queryBuffer.clear();
		for (HashGrid.Entity e : set) {
			queryBuffer.add(e);
		}
		Helpers.sortAndRemoveDuplicates(queryBuffer);

		PhysicsMath.SweepResult localSweepResult = new PhysicsMath.SweepResult();
		localSweepResult.point = new Vector2f();
		localSweepResult.normal = new Vector2f();
		Vector2f[] vertices = new Vector2f[] { new Vector2f(), new Vector2f() };
		Vector2f normal = new Vector2f();
		Vector2f movableEntityOffset = new Vector2f();
		Vector2f relativeVelocity = new Vector2f();

		sweepResult.t = Float.POSITIVE_INFINITY;
		for (HashGrid.Entity e : queryBuffer) {
			int type = e.getType();
			if (type == PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE) {
				// check if the contact is in the ignore list
				boolean ignore = false;
				for (ContactManager.MovementContact mc : movementContacts) {
					if (mc.type == Contact.Type.STATIC_COLLIDER && mc.index == (Integer) e.getData()) {
						ignore = true;
						break;
					}
				}
				if (ignore) {
					continue;
				}

				// add single contact for static collider
				Segment segment = simulation.staticColliders.get((Integer) e.getData());

				// TODO: could add initial check to see if the smallest possible t for this collider is greater than the current best t

				// sweep against segment
				PhysicsMath.sweepCapsuleAgainstSegment(d.shape, d.upVector, d.position, d.velocity,
						segment.getVerticesDirect(), segment.getNormalDirect(), localSweepResult);
				if (localSweepResult.t < sweepResult.t && localSweepResult.t <= dt) {
					// store this result
					sweepResult.t = localSweepResult.t;
					sweepResult.point.set(localSweepResult.point);
					sweepResult.normal.set(localSweepResult.normal);
				}
			} else if (type == PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE) {
				// add contacts for each edge in movable entity
				MovablePhysicsEntity movableEntity = (MovablePhysicsEntity) e.getData();
				// offset the movable entity into current timestep by velocity*t
				movableEntityOffset.set(movableEntity.getVelocity());
				movableEntityOffset.multiplyAssign(t);
				Polygon shape = movableEntity.getShape();

				// sweep with velocity relative to the movable entity
				relativeVelocity.set(d.velocity);
				relativeVelocity.subtractAssign(movableEntity.velocity);

				// check each edge
				for (int i = 0; i < shape.getVertexCount(); ++i) {
					boolean ignore = false;
					for (ContactManager.MovementContact mc : movementContacts) {
						if (mc.type == Contact.Type.MOVABLE_ENTITY && mc.index == i) {
							ignore = true;
							break;
						}
					}
					if (ignore) {
						continue;
					}

					int i1 = (i+1 == shape.getVertexCount()) ? 0 : i+1;
					vertices[0].set(shape.getVertexDirect(i));
					vertices[1].set(shape.getVertexDirect(i1));
					normal.set(shape.getNormalDirect(i));
					// transform the vertices
					// TODO could optimize by transforming capsule by inverse; might not be worth it
					Helpers.transformDirect(vertices[0], movableEntity.rotationVector, movableEntity.position);
					Helpers.transformDirect(vertices[1], movableEntity.rotationVector, movableEntity.position);
					Helpers.transformDirect(normal, movableEntity.rotationVector, null);
					vertices[0].addAssign(movableEntityOffset);
					vertices[1].addAssign(movableEntityOffset);

					// sweep against segment
					PhysicsMath.sweepCapsuleAgainstSegment(d.shape, d.upVector, d.position, relativeVelocity,
							vertices, normal, localSweepResult);
					if (localSweepResult.t < sweepResult.t && localSweepResult.t <= dt) {
						// store this result
						sweepResult.t = localSweepResult.t;
						sweepResult.point.set(localSweepResult.point);
						sweepResult.normal.set(localSweepResult.normal);
					}
				}
			}
		}
	}

	// constraintIndices is an array of size 2
	// it is filled with the indices in uniqueMovementContacts of the constraint applied (or -1 if less than 2 are appled)
	private boolean computeConstrainedVelocity(Vector2f velocity, Vector2f outVelocity, int[] constraintIndices) {
		constraintIndices[0] = constraintIndices[1] = -1;
		List<ContactManager.MovementContact> uniqueMovementContacts = contactManager.getUniqueMovementContacts();

		// first compute relative velocities
		for (ContactManager.MovementContact mc : uniqueMovementContacts) {
			mc.relativeVelocity.set(mc.velocity);
			mc.relativeVelocity.subtractAssign(velocity);
		}
		// now, instead of constraining velocity against contact.velocity,
		// we constrain 0 against contact.relativeVelocity

		// the constrained velocity is the velocity which takes "the path of least resistance"
		// (this makes more sense in 3D, but the idea is the same here)
		// that is, if we can find an outVelocity which is only constrained by one contact and does not conflict with
		// the other contacts, we use that result instead of one which is constrained by multiply contacts
		// start at no constraints and work our way up until we find a valid constrained velocity

		// check unconstrained velocity
		{
			outVelocity.setZero(); // when unconstrained, velocity is 0

			boolean pass = true;
			for (ContactManager.MovementContact mc : uniqueMovementContacts) {
				if (!checkContactAgainstVelocity(mc, outVelocity)) {
					pass = false;
					break;
				}
			}
			if (pass) {
				// add back velocity
				outVelocity.set(velocity);
				return true;
			}
		}

		// constrain against one contact
		for (int i = 0; i < uniqueMovementContacts.size(); ++i) {
			ContactManager.MovementContact constraint = uniqueMovementContacts.get(i);
			constrain1(constraint, outVelocity);

			boolean pass = true;
			for (ContactManager.MovementContact mc : uniqueMovementContacts) {
				if (constraint == mc) {
					continue;
				}
				if (!checkContactAgainstVelocity(mc, outVelocity)) {
					pass = false;
					break;
				}
			}
			if (pass) {
				// add back velocity
				outVelocity.addAssign(velocity);
				constraintIndices[0] = i;
				return true;
			}
		}

		// constrain against two contacts
		for (int i1 = 0; i1 < uniqueMovementContacts.size(); ++i1) {
			ContactManager.MovementContact constraintA = uniqueMovementContacts.get(i1);
			for (int i2 = i1+1; i2 < uniqueMovementContacts.size(); ++i2) {
				ContactManager.MovementContact constraintB = uniqueMovementContacts.get(i2);

				if (!constrain2(constraintA, constraintB, outVelocity)) {
					continue;
				}

				boolean pass = true;
				for (ContactManager.MovementContact mc : uniqueMovementContacts) {
					if (constraintA == mc || constraintB == mc) {
						continue;
					}
					if (!checkContactAgainstVelocity(mc, outVelocity)) {
						pass = false;
						break;
					}
				}
				if (pass) {
					// add back velocity
					outVelocity.addAssign(velocity);
					constraintIndices[0] = i1;
					constraintIndices[1] = i2;
					return true;
				}
			}
		}

		// failed to constrain velocity
		return false;
	}

	private boolean checkContactAgainstVelocity(ContactManager.MovementContact movementContact, Vector2f velocity) {
		// make sure the velocity moves us at least as far as the hyperplane of the constraint
		float nDotV = MathOps.dot(movementContact.normal, velocity);
		float nDotRV = MathOps.dot(movementContact.normal, movementContact.relativeVelocity);
		return nDotV >= nDotRV;
	}

	// constrains "zero velocity" against a single contact
	private void constrain1(ContactManager.MovementContact movementContact, Vector2f outVelocity) {
		float proj = MathOps.dot(movementContact.normal, movementContact.relativeVelocity) +
				simulation.settings.contactThreshold * 0.5f; // push out a tiny bit extra
		outVelocity.set(movementContact.normal);
		outVelocity.multiplyAssign(proj);
	}

	// constrains "zero velocity" against two contacts
	private boolean constrain2(ContactManager.MovementContact movementContactA, ContactManager.MovementContact movementContactB,
	                           Vector2f outVelocity) {
		// we want to solve for the vector V such that for each contact i,
		// relativeVelocity[i] - V is a vector in the contact's hyperplane
		// each hyperplane is defined as n[i] . p = 0
		// we solve n[i] . (relativeVelocity[i] - V) = 0

		// nA . (vA - V) = 0 ==> nA.V = nA.vA
		// nB . (vB - V) = 0 ==> nB.V = nB.vB
		// notice that we now have equations for two lines
		// solve for the intersection point

		Vector2f nA = movementContactA.normal;
		Vector2f nB = movementContactB.normal;
		float cA = MathOps.dot(movementContactA.normal, movementContactA.relativeVelocity);
		float cB = MathOps.dot(movementContactB.normal, movementContactB.relativeVelocity);
		// write implicit line equations using homogeneous coordinate
		// nx*x + ny*y - c*w = 0 ==> (nx, ny, -c) . (x, y, w) = 0
		// take cross product
		// (nAx, nAy, -cA) x (nBx, nBy, -cB) = (x, y, w)
		float x = -(nA.y()*cB - nB.y()*cA);
		float y = -(cA*nB.x() - cB*nA.x());
		float w = nA.x()*nB.y() - nB.x()*nA.y();
		if (w == 0.0f) {
			return false;
		}
		x /= w;
		y /= w;
		outVelocity.set(x, y);
		return true;
	}

	// determines if the constrained velocity is "squished" between the two contacts
	private MovablePhysicsEntity determineSquish(ContactManager.MovementContact mcA,
	                                             ContactManager.MovementContact mcB) {
		// test each movable contact against the other
		if (mcA.type == Contact.Type.MOVABLE_ENTITY) {
			MovablePhysicsEntity m = (MovablePhysicsEntity) mcA.object;
			if (determineSquishInternal(mcA, mcB, m.minCosSquishAngle)) {
				return m;
			}
		}

		if (mcB.type == Contact.Type.MOVABLE_ENTITY) {
			MovablePhysicsEntity m = (MovablePhysicsEntity) mcB.object;
			if (determineSquishInternal(mcB, mcA, m.minCosSquishAngle)) {
				return m;
			}
		}

		return null;
	}

	private boolean determineSquishInternal(
			ContactManager.MovementContact movableContact, ContactManager.MovementContact otherContact,
	        float minCosSquishAngle) {
		// find the angle between the two contacts
		float cosInvAngle = MathOps.dot(movableContact.normal, otherContact.normal);
		// angle = pi - acos(cosInvAngle);
		// cos(angle) = cos(pi - acos(cosInvAngle)
		//            = cos(pi)cos(acos(cosInvAngle)) + sin(pi)sin(acos(cosInvAngle))
		//            = -1*cosInvAngle + 0
		float cosAngle = -cosInvAngle;
		if (cosAngle < minCosSquishAngle) {
			return false;
		}

		// take the velocity relative to the other contact
		Vector2f relVelocity = movableContact.velocity.subtract(otherContact.velocity);
		// we squish if this contact is moving "forward"
		return MathOps.dot(relVelocity, movableContact.normal) > 0.0f;
	}

	// only checks for proximity zone enter/exit
	private void preResolveDynamicInteractions() {
		// we are checking dynamic entities against proximity zones
		int maskA = 1 << PhysicsSimulation.GRID_DYNAMIC_ENTITY_TYPE;
		int maskB = 1 << PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE;

		// query grid for intersections
		HashGrid.CollisionPairSet set = simulation.hashGrid.getCollisionPairSet(maskA, maskB);
		pairQueryBuffer.clear();
		for (HashGrid.CollisionPair p : set) {
			pairQueryBuffer.add(p);
		}
		Helpers.sortAndRemoveDuplicates(pairQueryBuffer);

		// initially assume no intersections - turn them on one by one
		// this way we can tell which ones are no longer intersecting
		for (PhysicsSimulation.TrackedIntersectionListNode n : simulation.trackedIntersections.values()) {
			n.intersecting = false;
		}

		PhysicsMath.DistanceResult distance = new PhysicsMath.DistanceResult();
		distance.pointA = new Vector2f();
		distance.vectorAB = new Vector2f();
		for (HashGrid.CollisionPair p : pairQueryBuffer) {
			HashGrid.Entity entityA = p.getEntityA();
			HashGrid.Entity entityB = p.getEntityB();
			if (entityB.getType() == PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE) {
				// if one of them is a proximity zone, make it A and not B
				// we've already masked out the case where both would be proximity zones
				HashGrid.Entity temp = entityA;
				entityA = entityB;
				entityB = temp;
			}

			// proximity zone/dynamic entity interaction
			PhysicsProximityZone z = (PhysicsProximityZone) entityA.getData();
			DynamicPhysicsEntity e = (DynamicPhysicsEntity) entityB.getData();
			interactProximityZoneDynamicEntity(z, e, false);
		}

		callOnExitEvents();
	}

	private void resolveDynamicInteractions() {
		// we are checking dynamic entities against dynamic entities and proximity zones
		int maskA = 1 << PhysicsSimulation.GRID_DYNAMIC_ENTITY_TYPE;
		int maskB = maskA | (1 << PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE);

		// query grid for intersections
		HashGrid.CollisionPairSet set = simulation.hashGrid.getCollisionPairSet(maskA, maskB);
		pairQueryBuffer.clear();
		for (HashGrid.CollisionPair p : set) {
			pairQueryBuffer.add(p);
		}
		Helpers.sortAndRemoveDuplicates(pairQueryBuffer);

		// initially assume no intersections - turn them on one by one
		// this way we can tell which ones are no longer intersecting
		for (PhysicsSimulation.TrackedIntersectionListNode n : simulation.trackedIntersections.values()) {
			n.intersecting = false;
		}

		for (HashGrid.CollisionPair p : pairQueryBuffer) {
			HashGrid.Entity entityA = p.getEntityA();
			HashGrid.Entity entityB = p.getEntityB();
			if (entityB.getType() == PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE) {
				// if one of them is a proximity zone, make it A and not B
				// we've already masked out the case where both would be proximity zones
				HashGrid.Entity temp = entityA;
				entityA = entityB;
				entityB = temp;
			}

			if (entityA.getType() == PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE) {
				// proximity zone/dynamic entity interaction
				PhysicsProximityZone z = (PhysicsProximityZone) entityA.getData();
				DynamicPhysicsEntity e = (DynamicPhysicsEntity) entityB.getData();
				interactProximityZoneDynamicEntity(z, e, true);
			} else {
				// dynamic entity/dynamic entity interaction
				DynamicPhysicsEntity eA = (DynamicPhysicsEntity) p.getEntityA().getData();
				DynamicPhysicsEntity eB = (DynamicPhysicsEntity) p.getEntityB().getData();
				interactDynamicEntities(eA, eB);
			}
		}

		callOnExitEvents();
	}

	private void interactProximityZoneDynamicEntity(PhysicsProximityZone z, DynamicPhysicsEntity e, boolean callOnIntersect) {
		if (!z.exists() || !e.exists()) {
			return;
		}

		// check for intersection
		boolean intersects = false;
		switch (z.getShape()) {
			case RECTANGLE:
				intersects = PhysicsMath.intersectCapsuleAabb(e.getShape(), e.upVector, e.position,
						z.boundMin, z.boundMax);
				break;
			case CIRCLE:
				intersects = PhysicsMath.intersectCapsuleCircle(e.getShape(), e.upVector, e.position,
						z.center, z.radius);
				break;
			default:
				assert false;
		}

		if (intersects) {
			PhysicsSimulation.TrackedIntersection ti = new PhysicsSimulation.TrackedIntersection();
			ti.proximityZone = z;
			ti.dynamicEntity = e;
			PhysicsSimulation.TrackedIntersectionListNode n = simulation.trackedIntersections.get(ti);
			boolean enter; // enter means the intersection just started
			if (n == null) {
				n = new PhysicsSimulation.TrackedIntersectionListNode();
				n.proximityZone = z;
				n.dynamicEntity = e;
				n.intersecting = true;
				simulation.trackedIntersections.put(ti, n);

				// add to list in each object
				n.proximityZonePrev = null;
				n.proximityZoneNext = z.trackedIntersectionList;
				if (z.trackedIntersectionList != null) {
					z.trackedIntersectionList.proximityZonePrev = n;
				}
				z.trackedIntersectionList = n;

				n.dynamicEntityPrev = null;
				n.dynamicEntityNext = e.trackedIntersectionList;
				if (e.trackedIntersectionList != null) {
					e.trackedIntersectionList.dynamicEntityPrev = n;
				}
				e.trackedIntersectionList = n;

				enter = true;
			} else {
				// update intersecting to true
				n.intersecting = true;
				enter = false;
			}

			boolean validCategory = CategoryMask.doesMaskContainCategory(z.categoryMask, e.category);
			if (validCategory) {
				if (enter) {
					z.eventHandler.onEnter(z, e);
				}
				if (callOnIntersect && z.exists()) {
					z.eventHandler.onIntersect(z, e);
				}
			}
		}
	}

	private void interactDynamicEntities(DynamicPhysicsEntity eA, DynamicPhysicsEntity eB) {
		if (!eA.exists() || !eB.exists()) {
			return;
		}

		PhysicsMath.DistanceResult distance = new PhysicsMath.DistanceResult();
		distance.pointA = new Vector2f();
		distance.vectorAB = new Vector2f();

		// determine how much they are overlapping
		PhysicsMath.distanceCapsuleCapsule(
				eA.getShape(), eA.upVector, eA.position, eB.shape, eB.upVector, eB.position,
				distance);

		if (distance.distance >= 0.0f) {
			// they aren't overlapping
			return;
		}

		// call collision function if it exists
		PhysicsSimulation.CollisionEventHandler eventHandlerAB = simulation.getCategoryCollisionHandler(
				eA.getCategory(), eB.getCategory());
		PhysicsSimulation.CollisionEventHandler eventHandlerBA = simulation.getCategoryCollisionHandler(
				eB.getCategory(), eA.getCategory());

		// make sure we pass the parameters in the right order, depending on the handler
		if (eventHandlerAB != null) {
			eventHandlerAB.onCollision(eA, eB);
		} else if (eventHandlerBA != null) {
			eventHandlerBA.onCollision(eB, eA);
		}

		// if either was deleted, skip the repel step
		if (eA.exists() || eB.exists()) {
			return;
		}

		boolean collisionEnabled = simulation.isCategoryCollisionEnabled(eA.getCategory(), eB.getCategory());
		if (!collisionEnabled) {
			return;
		}

		// redetermine distance, since they might have moved
		PhysicsMath.distanceCapsuleCapsule(
				eA.getShape(), eA.upVector, eA.position, eB.shape, eB.upVector, eB.position,
				distance);

		// repel the entities if collision is enabled between them
		// if the AB vector is 0, the two entities are exactly on each other
		// in this case, there is no "well defined" result, so we just ignore it
		// however, this would be incredibly rare to actually occur
		if (!distance.vectorAB.isZero()) {
			// determine how much each entity is affected by their masses
			float influenceA = eB.mass / (eA.mass + eB.mass);
			float influenceB = 1.0f - influenceA;
			// we take whichever has a higher repel speed
			float repel = Math.max(eA.repelAcceleration, eB.repelAcceleration);
			influenceA *= repel;
			influenceB *= repel;

			// compute the repel amount for each entity
			// vectorAB points from A to B, so we negate it to move them apart
			Vector2f repelVec = new Vector2f(distance.vectorAB);
			repelVec.multiplyAssign(-influenceA);
			eA.currentRepelAcceleration.addAssign(repelVec);

			repelVec.set(distance.vectorAB);
			repelVec.multiplyAssign(influenceB);
			eA.currentRepelAcceleration.addAssign(repelVec);

			// todo (possibly): if the two entities are on the ground,
			// project the repel vectors onto the ground plane
			// this is to avoid them "popping" off the ground a bit
			// in practice this most likely won't be an issue
		}
	}

	private void callOnExitEvents() {
		simulation.trackedIntersectionLock = true;
		// call exit function for any tracked intersections that no longer intersect
		for (PhysicsSimulation.TrackedIntersectionListNode n : simulation.trackedIntersections.values()) {
			if (!n.intersecting && n.proximityZone.exists() && n.dynamicEntity.exists()) {
				PhysicsProximityZone z = n.proximityZone;
				DynamicPhysicsEntity e = n.dynamicEntity;
				n.unlink();
				boolean validCategory = CategoryMask.doesMaskContainCategory(z.categoryMask, e.category);
				if (validCategory && z.eventHandler != null) {
					z.eventHandler.onExit(z, e);
				}
			}
		}
		simulation.trackedIntersectionLock = false;

		// remove dead entries
		Iterator<Map.Entry<PhysicsSimulation.TrackedIntersection, PhysicsSimulation.TrackedIntersectionListNode>> it =
				simulation.trackedIntersections.entrySet().iterator();
		while (it.hasNext()) {
			Map.Entry<PhysicsSimulation.TrackedIntersection, PhysicsSimulation.TrackedIntersectionListNode> e =
					it.next();
			PhysicsSimulation.TrackedIntersectionListNode n = e.getValue();
			if (!n.intersecting) {
				it.remove();
			}
		}
	}

	public void castRay(PhysicsRaycast raycast) {
		if (raycast.radius < 0.0f) {
			throw new IllegalArgumentException("Invalid ray radius");
		}

		// raycasts can be recursive
		boolean oldRaycastLock = simulation.raycastLock;
		simulation.raycastLock = true;

		// TODO: would be more efficient to raycast directly against the grid cells

		// construct ray AABB
		Vector2f aabbMin = raycast.ray.getOrigin();
		Vector2f aabbMax = raycast.ray.getOrigin();
		aabbMin.x(aabbMin.x() - raycast.radius);
		aabbMin.y(aabbMin.y() - raycast.radius);
		aabbMax.x(aabbMax.x() + raycast.radius);
		aabbMax.y(aabbMax.y() + raycast.radius);
		sweepAabb(aabbMin, aabbMax, raycast.ray.getDirection(), aabbMin, aabbMax);

		// find possible intersections
		int mask = 0;
		if (raycast.testStaticColliders) {
			mask |= (1 << PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE);
		}
		if (raycast.testDynamicEntities) {
			mask |= (1 << PhysicsSimulation.GRID_DYNAMIC_ENTITY_TYPE);
		}
		if (raycast.testMovableEntities) {
			mask |= (1 << PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE);
		}
		if (raycast.testProximityZones) {
			mask |= (1 << PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE);
		}
		HashGrid.CollisionAabbSet set = simulation.hashGrid.getCollisionAabbSet(mask, aabbMin, aabbMax);
		queryBuffer.clear();
		for (HashGrid.Entity e : set) {
			queryBuffer.add(e);
		}
		Helpers.sortAndRemoveDuplicates(queryBuffer);

		int raycastBufferStart = raycastBuffer.size();

		// build up buffer of the objects actually hit
		Vector2f[] vertices = new Vector2f[] { new Vector2f(), new Vector2f() };
		Vector2f normal = new Vector2f();
		PhysicsMath.SweepResult lsr = new PhysicsMath.SweepResult();
		lsr.point = new Vector2f();
		lsr.normal = new Vector2f();
		PhysicsMath.SweepResult msr = new PhysicsMath.SweepResult();
		msr.point = new Vector2f();
		msr.normal = new Vector2f();
		for (HashGrid.Entity e : queryBuffer) {
			int lsrIndex = -1;
			PhysicsRaycast.Result.IntersectionType lsrType = PhysicsRaycast.Result.IntersectionType.NONE;
			switch (e.getType()) {
				case PhysicsSimulation.GRID_STATIC_COLLIDER_TYPE:
				{
					int idx = (Integer) e.getData();
					Segment segment = simulation.getStaticCollider(idx);
					if (raycast.radius == 0.0f) {
						PhysicsMath.sweepPointAgainstSegment(
								raycast.ray,
								segment.getVerticesDirect(), segment.getNormalDirect(),
								lsr);
					} else {
						PhysicsMath.sweepCircleAgainstSegment(
								raycast.radius, raycast.ray.getOrigin(), raycast.ray.getDirection(),
								segment.getVerticesDirect(), segment.getNormalDirect(),
								lsr);
					}
					lsrIndex = idx;
					lsrType = PhysicsRaycast.Result.IntersectionType.SCENE;
					break;
				}
				case PhysicsSimulation.GRID_DYNAMIC_ENTITY_TYPE:
				{
					DynamicPhysicsEntity dpe = (DynamicPhysicsEntity) e.getData();
					if (CategoryMask.doesMaskContainCategory(raycast.categoryMask, dpe.category)) {
						PhysicsMath.sweepCircleAgainstCapsule(
								raycast.ray, raycast.radius,
								dpe.shape, dpe.upVector, dpe.position,
								lsr);
						lsrType = PhysicsRaycast.Result.IntersectionType.DYNAMIC_ENTITY;
					}
					break;
				}
				case PhysicsSimulation.GRID_MOVABLE_ENTITY_TYPE:
				{
					MovablePhysicsEntity mpe = (MovablePhysicsEntity) e.getData();
					Polygon shape = mpe.getShape();
					// find collider in shape with best t
					lsr.t = Float.POSITIVE_INFINITY;
					for (int i = 0; i < shape.getVertexCount(); ++i) {
						int i1 = (i+1 == shape.getVertexCount()) ? 0 : i+1;
						vertices[0].set(shape.getVertexDirect(i));
						vertices[1].set(shape.getVertexDirect(i1));
						normal.set(shape.getNormalDirect(i));
						Helpers.transformDirect(vertices[0], mpe.rotationVector, mpe.position);
						Helpers.transformDirect(vertices[1], mpe.rotationVector, mpe.position);
						Helpers.transformDirect(normal, mpe.rotationVector, null);
						if (raycast.radius == 0.0f) {
							PhysicsMath.sweepPointAgainstSegment(
									raycast.ray,
									vertices, normal,
									msr);
						} else {
							PhysicsMath.sweepCircleAgainstSegment(
									raycast.radius, raycast.ray.getOrigin(), raycast.ray.getDirection(),
									vertices, normal,
									msr);
						}
						if (msr.t < lsr.t) {
							lsr.t = msr.t;
							lsr.point.set(msr.point);
							lsr.normal.set(msr.normal);
							lsrIndex = i;
						}
					}
					lsrType = PhysicsRaycast.Result.IntersectionType.MOVABLE_ENTITY;
					break;
				}
				case PhysicsSimulation.GRID_PROXIMITY_ZONE_TYPE:
				{
					PhysicsProximityZone ppz = (PhysicsProximityZone) e.getData();
					switch (ppz.getShape()) {
						case RECTANGLE:
							PhysicsMath.sweepCircleAgainstAabb(
									raycast.ray, raycast.radius,
									ppz.boundMin, ppz.boundMax,
									lsr);
							break;
						case CIRCLE:
							PhysicsMath.sweepCircleAgainstCircle(
									raycast.ray, raycast.radius,
									ppz.radius, ppz.center,
									lsr);
							break;
						default:
							assert false;
					}
					lsrType = PhysicsRaycast.Result.IntersectionType.PROXIMITY_ZONE;
					break;
				}
				default:
					assert false;
			}

			if (lsr.t <= 1.0f) {
				// there was an intersection - add to buffer
				PhysicsRaycast.Result result = new PhysicsRaycast.Result();
				result.intersectionType = lsrType;
				result.t = lsr.t;
				result.point = new Vector2f(lsr.point);
				result.normal = new Vector2f(lsr.normal);
				result.intersectionObject = e.getData();
				result.intersectionIndex = lsrIndex;
				raycastBuffer.add(result);
			}
		}

		int raycastBufferEnd = raycastBuffer.size();

		// sort the new section of the raycast buffer
		// we sort using a backed view of the list
		Collections.sort(raycastBuffer.subList(raycastBufferStart, raycastBufferEnd), new Comparator<PhysicsRaycast.Result>() {
			@Override
			public int compare(PhysicsRaycast.Result o1, PhysicsRaycast.Result o2) {
				return Float.compare(o1.t, o2.t);
			}
		});

		// perform callback on each object hit in order
		boolean done = false;
		for (int i = raycastBufferStart; !done && i < raycastBufferEnd; ++i) {
			PhysicsRaycast.Result result = raycastBuffer.get(i);
			boolean exists;
			switch (result.intersectionType) {
				case NONE:
					exists = false;
					assert false;
					break;
				case SCENE:
					exists = true;
					break;
				case DYNAMIC_ENTITY:
					exists = ((DynamicPhysicsEntity) result.intersectionObject).exists();
					break;
				case MOVABLE_ENTITY:
					exists = ((MovablePhysicsEntity) result.intersectionObject).exists();
					break;
				case PROXIMITY_ZONE:
					exists = ((PhysicsProximityZone) result.intersectionObject).exists();
					break;
				default:
					exists = false;
					assert false;
			}
			if (exists) {
				done = !raycast.eventHandler.onIntersect(result);
			}
		}

		// pop elements from raycast buffer
		while (raycastBuffer.size() > raycastBufferStart) {
			raycastBuffer.remove(raycastBuffer.size()-1);
		}

		simulation.raycastLock = oldRaycastLock;

		simulation.deleteQueued();
	}

	// still works if in and out are same
	private void sweepAabb(Vector2f aabbMinIn, Vector2f aabbMaxIn, Vector2f velocity,
	                       Vector2f aabbMinOut, Vector2f aabbMaxOut) {
		boolean negX = velocity.x() < 0.0f;
		boolean negY = velocity.y() < 0.0f;
		float minX = negX ? aabbMinIn.x() + velocity.x() : aabbMinIn.x();
		float minY = negY ? aabbMinIn.y() + velocity.y() : aabbMinIn.y();
		float maxX = negX ? aabbMaxIn.x() : aabbMaxIn.x() + velocity.x();
		float maxY = negY ? aabbMaxIn.y() : aabbMaxIn.y() + velocity.y();
		aabbMinOut.set(minX, minY);
		aabbMaxOut.set(maxX, maxY);
	}

	ContactManager contactManager;
	private PhysicsSimulation simulation;

	private ArrayList<HashGrid.Entity> queryBuffer;
	private ArrayList<HashGrid.CollisionPair> pairQueryBuffer;
	private ArrayList<PhysicsRaycast.Result> raycastBuffer;
}
